#include <iostream>
#include <fstream>
#include <vector>
#include <limits>
#include <cstdint>
// include ros dep.
#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <image_transport/image_transport.h>
#include <dynamic_reconfigure/server.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/Bool.h>

#include "tf/tf.h"
#include "tf/transform_datatypes.h"
#include <tf/transform_broadcaster.h>
// include pcl dep
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
// include opencv and eigen
#include <Eigen/Eigen>
#include "opencv2/highgui/highgui.hpp"
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <cv_bridge/cv_bridge.h>

#include "depth_render.cuh"

using namespace cv;
using namespace std;
using namespace Eigen;

int *depth_hostptr;
cv::Mat depth_mat;

// camera param
int width, height;
double fx, fy, cx, cy;

DepthRender depthrender;
ros::Publisher pub_depth;
ros::Publisher pub_color;
ros::Publisher pub_pcl_wolrd;

sensor_msgs::PointCloud2 local_map_pcl;
sensor_msgs::PointCloud2 local_depth_pcl;

ros::Subscriber odom_sub;
ros::Subscriber global_map_sub, local_map_sub, cam_pose_sub;

ros::Timer local_sensing_timer, estimation_timer;

bool has_global_map(false);
bool has_local_map(false);
bool has_cam_pos(false);

Matrix4d cam2world;

double sensing_horizon, sensing_rate, estimation_rate;
double _x_size, _y_size, _z_size;
double _gl_xl, _gl_yl, _gl_zl;
double _resolution, _inv_resolution;
int _GLX_SIZE, _GLY_SIZE, _GLZ_SIZE;

ros::Time last_odom_stamp = ros::TIME_MAX;
Eigen::Vector3d last_pose_world;

vector<float> cloud_data;

void render_currentpose();
void render_pcl_world();

void camPoseCb(const geometry_msgs::PoseStampedConstPtr &msg)
{
  cam2world.block<3, 3>(0, 0) = Eigen::Quaterniond(msg->pose.orientation.w,
                                                   msg->pose.orientation.x,
                                                   msg->pose.orientation.y,
                                                   msg->pose.orientation.z)
                                    .toRotationMatrix();
  cam2world.block<3, 1>(0, 3) = Eigen::Vector3d(msg->pose.position.x,
                                                msg->pose.position.y,
                                                msg->pose.position.z);
  last_odom_stamp = msg->header.stamp;
  has_cam_pos = true;
}

void renderSensedPoints(const ros::TimerEvent &event)
{
  if (!has_global_map && !has_local_map)
    return;

  if (!has_cam_pos)
    return;
  render_currentpose();
  render_pcl_world();
}

void rcvGlobalPointCloudCb(const sensor_msgs::PointCloud2 &pointcloud_map)
{
  if (has_global_map)
    return;

  ROS_WARN("Global Pointcloud received..");
  // load global map
  pcl::PointCloud<pcl::PointXYZ> cloudIn;
  pcl::PointXYZ pt_in;
  // transform map to point cloud format
  pcl::fromROSMsg(pointcloud_map, cloudIn);
  for (int i = 0; i < int(cloudIn.points.size()); i++)
  {
    pt_in = cloudIn.points[i];
    cloud_data.push_back(pt_in.x);
    cloud_data.push_back(pt_in.y);
    cloud_data.push_back(pt_in.z);
  }
  printf("global map has points: %d.\n", (int)cloud_data.size() / 3);
  // pass cloud_data to depth render
  depthrender.set_data(cloud_data);
  depth_hostptr = (int *)malloc(width * height * sizeof(int));

  has_global_map = true;
}

void rcvLocalPointCloudCb(const sensor_msgs::PointCloud2 &pointcloud_map)
{
  ROS_WARN("Local Pointcloud received..");
  // load local map
  pcl::PointCloud<pcl::PointXYZ> cloudIn;
  pcl::PointXYZ pt_in;
  // transform map to point cloud format
  pcl::fromROSMsg(pointcloud_map, cloudIn);

  if (cloudIn.points.size() == 0)
    return;
  for (int i = 0; i < int(cloudIn.points.size()); i++)
  {
    pt_in = cloudIn.points[i];
    Eigen::Vector3d pose_pt(pt_in.x, pt_in.y, pt_in.z);
    cloud_data.push_back(pose_pt(0));
    cloud_data.push_back(pose_pt(1));
    cloud_data.push_back(pose_pt(2));
  }
  // printf("local map has points: %d.\n", (int)cloud_data.size() / 3 );
  // pass cloud_data to depth render
  depthrender.set_data(cloud_data);
  depth_hostptr = (int *)malloc(width * height * sizeof(int));

  has_local_map = true;
}

void render_pcl_world()
{
  // for debug purpose
  pcl::PointCloud<pcl::PointXYZ> localMap;
  pcl::PointXYZ pt_in;

  Eigen::Vector3d pose_in_camera;
  Eigen::Vector3d pose_pt;

  for (int u = 0; u < width; u++)
    for (int v = 0; v < height; v++)
    {
      uint16_t depth_t = depth_mat.at<uint16_t>(v, u);
      double depth = depth_t * 0.001;

      if (depth == 0) continue;
      if (depth > sensing_horizon) continue;

      pose_in_camera(0) = (u - cx) * depth / fx;
      pose_in_camera(1) = (v - cy) * depth / fy;
      pose_in_camera(2) = depth;

      pt_in.x = pose_in_camera(0);
      pt_in.y = pose_in_camera(1);
      pt_in.z = pose_in_camera(2);

      localMap.points.push_back(pt_in);
    }

  localMap.width = localMap.points.size();
  localMap.height = 1;
  localMap.is_dense = true;

  pcl::toROSMsg(localMap, local_map_pcl);
  local_map_pcl.header.frame_id = "camera";
  local_map_pcl.header.stamp = last_odom_stamp;

  pub_pcl_wolrd.publish(local_map_pcl);
}

void render_currentpose()
{
  double this_time = ros::Time::now().toSec();

  Matrix4d cam_pose = cam2world.inverse();

  double pose[4 * 4];

  for (int i = 0; i < 4; i++)
    for (int j = 0; j < 4; j++)
      pose[j + 4 * i] = cam_pose(i, j);

  depthrender.render_pose(pose, depth_hostptr);
  depth_mat = cv::Mat::zeros(height, width, CV_16UC1);
  double min = 0.5;
  for (int i = 0; i < height; i++)
    for (int j = 0; j < width; j++)
    {
      uint16_t depth;
      int depth_tmp = depth_hostptr[i * width + j];
      if (depth_tmp < 0) {
        depth = 0;
        cout << "Error: Integer value out of range for uint16_t, depth < 0" << endl;
      }
      else if (depth_tmp > std::numeric_limits<uint16_t>::max()) {
        depth = std::numeric_limits<uint16_t>::max();
      }
      else {
        depth = static_cast<uint16_t>(depth_tmp);
      }

      depth_mat.at<uint16_t>(i, j) = depth;
    }

  // publish depth image
  cv_bridge::CvImage out_msg;
  out_msg.header.stamp = last_odom_stamp;
  out_msg.header.frame_id = "camera";
  out_msg.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
  out_msg.image = depth_mat.clone();
  pub_depth.publish(out_msg.toImageMsg());

  // publish color depth image
  cv::Mat normalizedDepthMap;
  cv::normalize(depth_mat, normalizedDepthMap, 0, 255, cv::NORM_MINMAX, CV_8UC1);
  cv::Mat falseColorMap;
  cv::applyColorMap(normalizedDepthMap, falseColorMap, cv::COLORMAP_RAINBOW);

  cv_bridge::CvImage cv_image_colored;
  cv_image_colored.header.frame_id = "camera";
  cv_image_colored.header.stamp = last_odom_stamp;
  cv_image_colored.encoding = sensor_msgs::image_encodings::BGR8;
  cv_image_colored.image = falseColorMap;
  pub_color.publish(cv_image_colored.toImageMsg());
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "pcl_render");
  ros::NodeHandle nh("~");

  nh.getParam("cam_width", width);
  nh.getParam("cam_height", height);
  nh.getParam("cam_fx", fx);
  nh.getParam("cam_fy", fy);
  nh.getParam("cam_cx", cx);
  nh.getParam("cam_cy", cy);
  nh.getParam("sensing_horizon", sensing_horizon);
  nh.getParam("sensing_rate", sensing_rate);
  nh.getParam("estimation_rate", estimation_rate);

  nh.getParam("map/x_size", _x_size);
  nh.getParam("map/y_size", _y_size);
  nh.getParam("map/z_size", _z_size);

  depthrender.set_para(fx, fy, cx, cy, width, height);

  // init cam2world transformation
  cam2world << 0.0, 0.0, 1.0, 0.0,
      -1.0, 0.0, 0.0, 0.0,
      0.0, -1.0, 0.0, 0.0,
      0.0, 0.0, 0.0, 1.0;

  // subscribe point cloud
  global_map_sub = nh.subscribe("global_map", 1, rcvGlobalPointCloudCb);
  local_map_sub = nh.subscribe("local_map", 1, rcvLocalPointCloudCb);
  cam_pose_sub = nh.subscribe("camera_pose", 10, camPoseCb);

  // publisher depth image and color image
  pub_depth = nh.advertise<sensor_msgs::Image>("depth", 1000);
  pub_color = nh.advertise<sensor_msgs::Image>("colordepth", 1000);
  pub_pcl_wolrd = nh.advertise<sensor_msgs::PointCloud2>("rendered_pcl", 1);

  double sensing_duration = 1.0 / sensing_rate;

  local_sensing_timer = nh.createTimer(ros::Duration(sensing_duration), renderSensedPoints);

  _inv_resolution = 1.0 / _resolution;

  _gl_xl = -_x_size / 2.0;
  _gl_yl = -_y_size / 2.0;
  _gl_zl = 0.0;

  _GLX_SIZE = (int)(_x_size * _inv_resolution);
  _GLY_SIZE = (int)(_y_size * _inv_resolution);
  _GLZ_SIZE = (int)(_z_size * _inv_resolution);

  ros::Rate rate(100);
  while (ros::ok())
  {
    ros::spinOnce();
    rate.sleep();
  }
}
